import numpy as np

class PIDController:

    def __init__(self, kp=50.0, ki=0.5, kd=0.3, u_max=1.0, u_min=-1.0):
        """
        初始化PID控制器
        
        Args:
            kp: 比例增益
            ki: 积分增益
            kd: 微分增益
            u_max: 控制输出上限
            u_min: 控制输出下限
        """
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.u_max = u_max
        self.u_min = u_min
        
        # 初始化内部状态
        self.error_prev = 0.0
        self.integral = 0.0
        
    def compute_control(self, error, dt):
        """
        计算PID控制输出
        
        Args:
            error: 当前误差
            dt: 时间步长
            
        Returns:
            control: 控制输出
        """
        # 计算微分项
        if dt > 0:
            derivative = (error - self.error_prev) / dt
        else:
            derivative = 0.0
            
        # 更新积分项
        self.integral += error * dt
        
        # 计算PID输出
        control = self.kp * error + self.ki * self.integral + self.kd * derivative
        
        # 限制控制输出范围
        control = np.clip(control, self.u_min, self.u_max)
        
        # 更新历史值
        self.error_prev = error
        
        return control
        
    def reset(self):
        """
        重置控制器内部状态
        """
        self.error_prev = 0.0
        self.integral = 0.0